/******************************************************************************
 * Copyright 2022 The Untouch Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <memory>

#include "Eigen/Dense"
#include "opencv2/opencv.hpp"
#include "pcl/common/transforms.h"
#include "pcl/io/pcd_io.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/pcl_base.h"

#include "modules/calibration/lidar_imu_calibrator/include/odometry_frame_set.h"
#include "cyber/common/log.h"
#include "modules/common/math/euler_angles_zxy.h"

namespace lidar_imu_calibrator {
using apollo::common::math::EulerAnglesZXYd;
typedef pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudPtr;
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
typedef pcl::PointXYZI Point;

class SelfLidarImuCalibrator {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Ptr = std::shared_ptr<SelfLidarImuCalibrator>;

 public:
  SelfLidarImuCalibrator();
  ~SelfLidarImuCalibrator() = default;

  double Calibrate(std::vector<OdometryFrame::Ptr>& frames, Eigen::Isometry3d& extrinsics);
  bool SaveCloud(const std::string &path, const Eigen::Isometry3d& extrinsics);

 private:
  bool SelectFrames(const std::vector<OdometryFrame::Ptr>& frames);
  void DrawPose(const Eigen::Isometry3d& pose, int mark_radius, const cv::Scalar& color);

  int PoseSelection(const std::vector<std::pair<double, double>> &positions, 
    const std::vector<double> &euler_yaw_pre_imu);

  bool IsCalibrate();
  bool IsStarSelect(const std::vector<double> &euler_yaw_pre_imu);

  double StarCalibration(const std::vector<OdometryFrame::Ptr>& frames, Eigen::Isometry3d& extrinsics);


  double Optimize(const std::vector<double>& lb, const std::vector<double>& ub,
                SelfLidarImuCalibrator* opt_data, std::vector<double>* x);

  static double LidarOdomMinimizer(const std::vector<double>& x,
                            std::vector<double>& grad, void* f_data);

  void GetTwoCombinedPointcloud(const Eigen::Isometry3d& extrinsics,
                                PointCloud* merged_cloud_1,
                                PointCloud* merged_cloud_2);

  float lidarOdomKNNError(const PointCloud& base_pointcloud,
                          const PointCloud& combined_pointcloud);
  float lidarOdomKNNError();

  static float kNNError(
      const pcl::KdTreeFLANN<Point>& kdtree, const PointCloud& pointcloud,
      const size_t k, const float max_dist, const size_t start_idx,
      const size_t end_idx);

  void SetTransformLidar2Imu(const Eigen::Matrix<double, 6, 1> &vec);

  void Euler2Isometry3d(const Eigen::Matrix<double, 6, 1> &vec, Eigen::Isometry3d& extrinsics);
 private:
  std::vector<OdometryFrame::Ptr> select_frames_;
  std::vector<std::pair<int, int>> select_pairs_positive_, select_paris_reverse_;
  Eigen::Isometry3d T_l_o_, base_pose_;
  int first_circle_idex_ = 0;

  double max_angle_cumulative_ = 400;
  double time_limit_ = 1;
  double min_dist_ = 1;
  double bound_forward_ = 22.5;
  int max_bin_size_ = 20;
  double translation_range_ = 0.1, angular_range_ = 5;

  double max_evals_ = 1000;
  double xtol_ = 0.00001;
  int knn_batch_size_ = 100000;
  int knn_k_ = 1;
  float local_knn_max_dist_ = 1.0;
  double max_time_offset_ = 0.1;
  bool time_cal_ = false;
  bool star_select_frames_ = false;

  cv::Mat draw_board_;
};
}  // namespace lidar_imu_calibrator
